/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include "replicate_short_to_full_calc_cor_eng_adapt.h"
#include "main.h"

/*
 *  I combined ReplicateShortToFull with CalcCorEngAdapt to elimate
 *  memory accesses when ReplicateShortToFull is called.
 *
 *  - DH
 */

/**************************************************************************
*                                                                         *
* ROUTINE
*               ReplicateShortToFull
* FUNCTION
*               Replicate the short adaptive codeword to the full codeword
*               length
* SYNOPSIS
*               ReplicateShortToFull(Exc_len, delay, Conv, FullConv)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       Exc_len         int      i      Length of excitation vector
*       delay           int      i      Pitch lag
*       Conv            fxpt_16  i      Truncated convolved impulse response
*       FullConv        fxpt_16  o      Replicated convolved impulse response
*
**************************************************************************/

/**************************************************************************
*                                                                         *
* ROUTINE
*               CalcCorEngAdapt
* FUNCTION
*               Calculate Correlation and Energy
* SYNOPSIS
*               CalcCorEngAdapt(AnaSignal, MatchSignal, length,
*                       correlation, energy)
*
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       AnaSignal       fxpt_16  i      Signal to analyze
*       MatchSignal     fxpt_16  i      Signal to match
*       length          int      i      Length of signals
*       correlation     fxpt_32  o      Correlation between to signals
*       energy          fxpt_32  o      Energy in signal to analyze
*
*
**************************************************************************/

void ReplicateShortToFullCalCorEngAdapt(
  int           delay,                  /* must be < Exc_len */
  fxpt_32       Conv[],                 /* 18.13 format */
  fxpt_16       MatchSignal[],          /* 15.0 format */
  fxpt_32       *correlation,           /* 30.1 format */
  fxpt_32       *energy)                /* 30.1 format */
{
  int           i;
  fxpt_32       t1, t2;
  int           delay_times_2;
  int64         ener_acc;
  int64         corr_acc;

  ener_acc = 0;
  corr_acc = 0;

  for (i = 0; i < delay; i++) {
    t1 = Conv[i];
    t2 = MatchSignal[i];
    ener_acc += (int64)t1 * t1;
    corr_acc += (int64)t1 * t2;
  }

  delay_times_2 = delay * 2;

  if (delay_times_2 < SF_LEN) {

    /*  Add in 2nd convolution */
    for (i = delay; i < delay_times_2; i++) {
      t1 = Conv[i] + Conv[i-delay];
      t2 = MatchSignal[i];
      ener_acc += (int64)t1 * t1;
      corr_acc += (int64)t1 * t2;
    }

    /* Add in 3rd convolution -- if any */
    for (i = delay_times_2; i < SF_LEN; i++) {
      t1 = Conv[i] + Conv[i-delay];
      t1 += Conv[i-2*delay];
      t2 = MatchSignal[i];
      ener_acc += (int64)t1 * t1;
      corr_acc += (int64)t1 * t2;
    }
  }
  else {
    for (i = delay; i < SF_LEN; i++) {
      t1 = Conv[i] + Conv[i-delay];
      t2 = MatchSignal[i];
      ener_acc += (int64)t1 * t1;
      corr_acc += (int64)t1 * t2;
    }
  }
//  *energy = ener_acc >> 3;
//  *correlation = corr_acc >> 3;
  *energy = fxpt_saturate32_round( ener_acc, 29 );
  *correlation = fxpt_saturate32_round( corr_acc, 16 );
}
